# -*- coding: utf-8 -*-
import logging
import math
import os
import sqlite3
import struct
import time
import serial
from threading import Thread
from serial import Serial

import config

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)
####
#com1 六轴机器人#com2 四轴机器人 
####
class robot_control:

    def __init__(self) -> None:
        # 定义一个端口列表  
        ports = ['/dev/ttyUSB0', '/dev/ttyUSB1']  
        # 尝试打开端口  
        self.serial_com2 = self.try_serial_ports(ports) 

        self.thread_update = Thread(target=self.task_com2,)
        self.thread_update.start()
        self.status_global_ = None
        time.sleep(0.01)
        
        self.serial_com1 = None  
        try:  
            self.serial_com1 = serial.Serial("/dev/ttyS7", baudrate=115200, timeout=1)  
        except serial.SerialException as e:  
            print(f"Failed to open port /dev/ttyS7: {e}")  
  
        if self.serial_com1:  
            self.thread_update_ = Thread(target=self.task_com1)  
            self.thread_update_.start()  

        self.status_global = None
        self.default_speed = 2000
        # 这里来表示机器人类型。0：代表无机器人，1:代表是六轴，2：代表是四轴
        self.robot_type_number = 0 
        self.robot_type_number_2 = 0

        # 定义RTU服务内容
        self.rtu_slave = 0
        self.rut_en = 0

        ########################数据库存储状态############################
        filename = 'ktr6.db'
        filename_ = 'ktr4.db'
        file_dir = "/home/cat/gm/data/"
        # # 使用系统分隔符连接目录和文件
        # file_path = os.sep.join([file_dir, filename])
        # 使用路径模块连接
        file_path = os.path.join(file_dir, filename)
        file_path_ = os.path.join(file_dir, filename_)
        
        self.conn = sqlite3.connect(file_path,timeout=10,check_same_thread=False)
        self.conn_  = sqlite3.connect(file_path_,timeout=10,check_same_thread=False)

    def try_serial_ports(self, ports):  
        for port in ports:  
            try:  
                ser = Serial(port, 115200, timeout=0.5)
                time.sleep(0.91)
                print(f"Successfully opened {port}")  
                return ser  
            except serial.serialutil.SerialException as e:  
                print(f"Failed to open {port}: {e}")  
                time.sleep(0.1)  # 等待一秒再尝试下一个端口  
        return None  

    def send_msg(self,message, wait_ok=False, wait_idle=True , terminator=os.linesep, com_flag = None):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # 串口发送数据，编码为utf-8
            if com_flag == 1:
                self.serial_com1.write(message.encode('utf-8'))
            elif com_flag == 2:
                self.serial_com2.write(message.encode('utf-8'))
            
            return True
        except Exception as e:
            # 日志写入串口设备写入异常
            
            robot_logger.error(e,exc_info=True)
            return False

    def robot_type_update(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            
            if com_flag == 1:
                self.serial_com1.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=1)
                time.sleep(0.1)
                data = self.serial_com1.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com1:" + recive)
            # time.sleep(0.01)
            
            # if "Ry" in recive:
            if "Mirobot" in recive:
                self.robot_type_number = 1
            elif "E4" in recive:
                self.robot_type_number = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update_2(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            
            if com_flag == 2:
                self.serial_com2.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=2)
                time.sleep(0.1)
                data = self.serial_com2.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com2:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number_2 = 1
            elif "E4" in recive:
                self.robot_type_number_2 = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                # print(f"2024--7-15 status_data is {status_date}")
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number

                    }
            
            if status != None:

                self.status_global = status
                self.send_tcp_data(status)

            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json_2(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                # print(f"2024--7-15 status_data com2 is {status_date}")
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number_2

                    }
            
            if status != None:
                    self.status_global_ = status
                    self.send_tcp_data_2(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)
  
    ### 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    #### 数据---TCP
    def send_tcp_data(self,data):
        try:
            j0  = data["angle"][0]
            j1  = data["angle"][1]
            j2  = data["angle"][2]
            j3  = data["angle"][3]
            j4  = data["angle"][4]
            j5  = data["angle"][5]
            j6  = data["coordinate"][6]


            # robotdata_byte0=(self.float2bytes(j0))
            # robotdata_byte1=(self.float2bytes(j1))
            # robotdata_byte2=(self.float2bytes(j2))
            # robotdata_byte3=(self.float2bytes(j3))
            # robotdata_byte4=(self.float2bytes(j4))
            # robotdata_byte5=(self.float2bytes(j5))
            # robotdata_byte6=(self.float2bytes(j6))
            
            ###将MS数据存入字典
            config.set_value("j0",j0) 
            config.set_value("j1",j1)
            config.set_value("j2",j2)
            config.set_value("j3",j3)
            config.set_value("j4",j4)
            config.set_value("j5",j5)
            config.set_value("FO[1]",value=j6)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def send_tcp_data_2(self,data):
        try:
            j7  = data["angle"][0]
            j8  = data["angle"][1]
            j9  = data["angle"][2]
            j10  = data["angle"][3]

            # robotdata_byte7=(self.float2bytes(j7))
            # robotdata_byte8=(self.float2bytes(j8))
            # robotdata_byte9=(self.float2bytes(j9))
            # robotdata_byte10=(self.float2bytes(j10))

            config.set_value("j7",j7) 
            config.set_value("j8",j8)
            config.set_value("j9",j9)
            config.set_value("j10",j10)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def set_robot_state(self,data):
        try :
            
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=1")
            data_en = en.fetchall()

            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state where ID=1")
                data_gcode = gcode.fetchall()
                print(f"gcode is {data_gcode[0][0]}")
                self.send_msg(data_gcode[0][0], com_flag= 1)
                c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                # self.conn.commit() 
            self.rtu_slave = data_en[0][1]
            self.rut_en = data_en[0][2]  # 更新服务状态
            # 将机器人状态更新到数据库里。
            if state=="Idle":
                c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
            elif state=="Alarm":

                c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
            elif state=="Home":

                c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
            elif state=="Run":

                c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")

            self.conn.commit() 
        except Exception as e:
            print(f"set_robot_state error is {e}")


    def set_robot_state_(self,data):
        try :
            c = self.conn_.cursor()
            # print(1)
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state2 where ID=1")
            # print(2)
            data_en = en.fetchall()
            # print(3)

            # 检测是否有G代码要执行
            if data_en[0][0] == 1:
                gcode = c.execute("SELECT GCODE from Robot_state2 where ID=1")
                # print(4)
                
                data_gcode = gcode.fetchall()
                print(f"gcode_ is {data_gcode[0][0]}")
                self.send_msg(data_gcode[0][0], com_flag= 2)
                # print(5)
                c.execute("UPDATE Robot_state2 set EN = 0 where ID=1")
                # self.conn.commit() 
            self.rtu_slave = data_en[0][1]
            self.rut_en = data_en[0][2]  # 更新服务状态
            # 将机器人状态更新到数据库里。
            # 更新状态  
            # if state in ["Idle", "Alarm", "Home", "Run"]:  
            #     print(6)
            #     c.execute("UPDATE Robot_state2 set STATE = ? where ID=1 ", (state))

            if state=="Idle":
                # print(f"6-1")
                c.execute("UPDATE Robot_state2 set STATE = 'Idle' where ID=1")
            
            elif state=="Alarm":
                # print(f"6-2")
                c.execute("UPDATE Robot_state2 set STATE = 'Alarm' where ID=1")
            
            elif state=="Home":
                # print(f"6-3")
                c.execute("UPDATE Robot_state2 set STATE = 'Home' where ID=1")
            
            elif state=="Run":
                # print(f"6-4")
                c.execute("UPDATE Robot_state2 set STATE = 'Run' where ID=1") 

            # print(7)
            self.conn_.commit() 
        except Exception as e:
            print(f"set_robot_state_ error is {e}")

    #### 六轴
    def task_com1(self,):
        type_flag_count = 0   
        while True:
            try:
                gcode = f"?\r\n"
                self.send_msg(gcode, com_flag=1)
                time.sleep(0.1)
                try:
                    ### 从串口读取数据 
                    data = self.serial_com1.read_all()
                    # print(f"task_com1  Gcode ? recv data is {data}")
                except serial.SerialException as e:  
                    print(f"serial_com2.read_all: {e}")
                
                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number == 0:  
                        self.robot_type_update(com_flag=1)

                else:
                    type_flag_count +=1

                    # print(type_flag_count)

                    if type_flag_count == 10:
                        self.robot_type_number = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json(data)
                    self.set_robot_state(self.status_global["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
    #### 四轴
    def task_com2(self,):
        type_flag_count = 0
        while True:
            try:
                gcode = f"?\r\n"
                self.send_msg(gcode, com_flag=2)
                time.sleep(0.1)
                try:
                    ### 从串口读取数据 
                    data = self.serial_com2.read_all()
                    # print(f"task_com2  Gcode ? recv data is {data}")
                except serial.SerialException as e:  
                    print(f"serial_com2.read_all: {e}") 
                

                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number_2 == 0:  
                        self.robot_type_update_2(com_flag=2)

                else:
                    type_flag_count +=1
                    if type_flag_count == 10:
                        self.robot_type_number_2 = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json_2(data)
                    self.set_robot_state_(self.status_global_["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
    
    #### 机器人Gcode运动控制指令
    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True, com_flag = 1)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        # msg = f'!'
        # self.send_msg(msg, wait_ok=False, wait_idle=False)
        # time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value
    
    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)
    
    def move(self, P, speed = None ,coordinate= 1, is_relative=0,mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'D':P[6], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True, com_flag = 1)

    def circular_interpolation(self, ex, ey, radius, is_cw=True, speed=None, wait_ok=None):
        '''圆弧插补
        在XY平面上, 从当前点运动到相对坐标(ex, ey).半径为radius
        `is_cw`决定圆弧是顺时针还是逆时针.
        '''
        # 判断是否合法
        distance = math.sqrt(ex**2 + ey**2)
        if distance > (radius * 2):
            self.logger.error(f'circular interpolation error, target posi is too far')
            return False

        instruction = None
        if is_cw:
            instruction = 'M20 G91 G02'
        else:
            instruction = 'M20 G91 G03'
        
        pairings = {'X': ex, 'Y': ey, 'R': radius, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def set_door_lift_distance(self, lift_distance):
        '''设置门式轨迹规划抬起的高度'''
        msg = f"$49={lift_distance}"
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def door_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, wait_ok=None):
        '''门式插补'''
        instruction = 'M20 G90 G05'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G05'
        
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

